#include "common.h"

std::vector<std::string> CommonFunc::split(std::string str,std::string pattern)
{
	std::string::size_type pos;
	std::vector<std::string> result;
	str+=pattern;
	int size=str.size();

	for(int i=0; i<size; i++)
	{
		pos=str.find(pattern,i);
		if(pos<size)
		{
			std::string s=str.substr(i,pos-i);
			result.push_back(s);
			i=pos+pattern.size()-1;
		}
	}
	return result;
}
geometry_msgs::Vector3 CommonFunc::qurt2euler(const geometry_msgs::Quaternion msg)
{
	tf::Quaternion quat;
	tf::quaternionMsgToTF(msg, quat);

	// the tf::Quaternion has a method to acess roll pitch and yaw
	double roll, pitch, yaw;
	tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

	// the found angles are written in a geometry_msgs::Vector3
	geometry_msgs::Vector3 rpy;
	rpy.x = roll;
	rpy.y = pitch;
	rpy.z = yaw;
	return rpy;
}
void CommonFunc::rolateWithAxis(const float theta,const int orginPos[2],int goalPos[2])
{
	goalPos[0] = orginPos[0]*cos(theta) + orginPos[1]*sin(theta);
	goalPos[1] = -orginPos[0]*sin(theta) + orginPos[1]*cos(theta);
}
void CommonFunc::rolateByAxis(const float theta,const int orginPos[2],int goalPos[2])
{
	goalPos[0] = orginPos[0]*cos(theta) - orginPos[1]*sin(theta);
	goalPos[1] = orginPos[0]*sin(theta) + orginPos[1]*cos(theta);
}

float CommonFunc::angle2_pi_pi(float angle)
{
	while(angle<=-PI)
		angle += 2*PI;
	while(angle>PI)
		angle -= 2*PI;
	return  angle;
}
float CommonFunc::angle2_2pi(float angle)
{
	while(angle<=0)
		angle += 2*PI;
	while(angle>2*PI)
		angle -= 2*PI;
	return angle;
}

void CommonFunc::bresenham(int x1,int y1,int x2,int y2,vector<int> *grid_free_x,vector<int> *grid_free_y)
{
	if (x1 != x2)
	{
		float k = float(y2-y1)/float(x2-x1);
		int flag = 0;
		if (abs(k)>1.)
		{
			flag=1;
			k=1./k;
			int temp;
			temp = x1;	x1=y1;	y1=temp;// 	[y1 x1]=Swap(x1,y1);           
			temp = x2;	x2=y2;	y2=temp;//	[y2 x2]=Swap(x2,y2);
		}
		int mi = (x1<x2)?x1:x2;
		int ma = (x1>x2)?x1:x2;
		int s;
		if (mi == x1)
			s = y1;
		else
			s = y2;
		float d = 0;
		for(int i=mi;i<ma;i++)
		{
			if (0 == flag)
			{
				grid_free_x->push_back(i);
				grid_free_y->push_back(s);
			}
			else
			{
				grid_free_x->push_back(s);
				grid_free_y->push_back(i);
			}
			d = d+k;
			if(d>=1.)
			{
				d=d-1.;
				s=s+1.;
	
			}
			else if(d<=-1.)
			{
				d=d+1.;
				s=s-1.;
			}
		}
	}
	else
	{
		int sign = (y1<y2)?1:-1;
		for(int i=0;i<abs(y2-y1);i++)
		{
			grid_free_x->push_back(x1);
			grid_free_y->push_back(y1+sign*i);
		}
	}
}